#ifndef __PID2P2Z_H__
#define __PID2P2Z_H__


typedef struct {
    float bn[3];
    float xn[3];
    float yn[2];
    float windup_limit;
}PidController2p2z;

typedef struct {
    float kp;
    float ki;
    float kd;
    float windup_limit;
} PidControllerSettings;

/**
 * @brief initiliaze Pid controller based on 2p2z approach
 * @param self 2p2z pid controller object
 * @param settings pid controllers initial settings
 * @return 0 on success, otherwise -1
 */ 
int PidController2p2zInit(PidController2p2z *self, PidControllerSettings *settings);
float PidController2p2zUpdate (PidController2p2z *self, float reference, float measured);
void PidController2p2zTune (PidController2p2z *self, float kp, float ki, float kd);
void PidController2p2zReset (PidController2p2z *self);

#if 0

#include "pid2p2z.h"

//PI controller sampled at 1ms
#define SAMPLE_RATE_US 1000
#define P_GAIN         10.0f
#define I_GAIN         (0.1f * (1.0f / (float) SAMPLE_RATE_US))

static PidController2p2z pi2p2z;
static PidControllerSettings settings;


static float StateEstimator(void) {
   //Add your 
   return 0.0f;
}

static void Act(float command) {
    (void)command;
    //TODO: add your actuation code here
}


void PidController2p2zdemo(void)
{
    float dummy_reference = 1.0f;
	
    //On PI controllers there is no derivative gain.
    settings.kd = 0.0f;
    settings.kp = P_GAIN;
    settings.ki = I_GAIN;
    settings.windup_limit = 1000.0f;
    //FIRST: Initialize PID object with desired settings:
    PidController2p2zInit(&pi2p2z, &settings);

    //Simulate a closed loop control:
    while(1) {
        //THIRD MEasure sensors and compute controller:
        float command = PidController2p2zUpdate(&pi2p2z, dummy_reference, StateEstimator ());

        //OPTIONAL: reset or tune PID in runtime:
        // PidController2p2zTune(&pi2p2z, new_kp, new_ki, new_kd);
        // PidController2p2zReset(&pi2p2z);

        //FOURTH: Update + scale command to your actuator:
        Act(command);
        usleep(SAMPLE_RATE_US);
    }
}

#endif

#endif
